//
// Created by 74354 on 2022/1/11.
//

#include "foc.h"
#include "commuTask.h"
#include "main.h"
#include "tim.h"
#include "cmsis_os.h"

union float_data
{
    float fdata;
    uint8_t  byte[4];
};

void FOC::runToAngle(float angle)
{
    u_alpha = cos(PI*angle/180.0f);
    u_beta = sin(PI*angle/180.0f);
    svpwm();
}

/* current_control
 *
*/
void FOC::current_control(float iq_ref,float id_ref)
{
    backward();
    forward(iq_ref,id_ref);
}

void FOC::positionControl(float mRadianNext)
{
    float mRadianNow,mRadError,intalTemp,diffTemp,gainTemp,output;
    while(true)
    {
        mRadianNow = getMachineRadian();
        mRadError = mRadianNext - mRadianNow;
        if(mRadError < 0.1 && mRadError > -0.1)break;
        if(mRadError < 1 && mRadError > -1)posControl.integral += mRadError*posControl.i_gain;
        else posControl.integral = 0;

        intalTemp = posControl.integral;
        diffTemp = posControl.d_gain * (mRadError - posControl.lastError);
        gainTemp = posControl.p_gain * mRadError;
        output = intalTemp + diffTemp + gainTemp;
        posControl.lastError = mRadError;

        if(output > 2)output = 2;
        if(output < -2)output = -2;
        current_control(output,0);
        osDelay(1);
    }
}


void FOC::backward(){

    float i_alpha,i_beta;
    std::tuple<float,float,float> I_abc = getCurrentABC();
    clark(std::get<0>(I_abc),std::get<1>(I_abc),std::get<2>(I_abc),i_alpha,i_beta);
    park(i_alpha,i_beta,getElectricRadian(),i_d,i_q);

    // 对Iq 和 Id的数据进行卡尔曼滤波
    if(EN_KALMAN)
    {
        i_d = kalman_I_D->addValue(i_d);
        i_q = kalman_I_Q->addValue(i_q);
    }
}

/* @brief release: release the motor
 * 
 */
void FOC::release() {

    __HAL_TIM_SET_COMPARE(&driver_timer,TIM_CHANNEL_1,0);
    __HAL_TIM_SET_COMPARE(&driver_timer,TIM_CHANNEL_2,0);
    __HAL_TIM_SET_COMPARE(&driver_timer,TIM_CHANNEL_3,0);
}


void FOC::forward(float iq_ref, float id_ref) {


    float Ierr_d = id_ref - i_d;
    float Ierr_q = iq_ref - i_q;

    /* PI controller */
    // 比例环节
    float Vd = piControl.i_integral_d + Ierr_d * piControl.kp_D;
    float Vq = piControl.i_integral_q + Ierr_q * piControl.kp_Q;

    float mod_to_V = (2.0f / 3.0f) * u_dc;
    float V_to_mod = 1.0f / mod_to_V;
    float mod_d = V_to_mod * Vd;
    float mod_q = V_to_mod * Vq;

    float mod_scalefactor = 0.3f * sqrt3_by_2 * 1.0f / sqrtf(mod_d * mod_d + mod_q * mod_q);
    if (mod_scalefactor < 1.0f) {
        mod_d *= mod_scalefactor;
        mod_q *= mod_scalefactor;
        // TODO make decayfactor configurable
        piControl.i_integral_d *= 0.99f;
        piControl.i_integral_q *= 0.99f;
    } else {
        // 累加器增加，积分环节
        piControl.i_integral_d += Ierr_d * piControl.ki_D;
        piControl.i_integral_q += Ierr_q * piControl.ki_Q;
    }

    inv_park(mod_d,mod_q,getElectricRadian(),u_alpha,u_beta);

    u_alpha = mod_to_V * u_alpha;
    u_beta = mod_to_V * u_beta;
    if(u_alpha > 1)u_alpha = 1;
    else if(u_alpha < -1)u_alpha = -1;
    if(u_beta > 1)u_beta = 1;
    else if(u_beta < -1)u_beta = -1;

    svpwm();

}

void FOC::svpwm()
{
    float K = SQRT_3 * Ts / u_dc;
    float U1 = u_beta;
    float U2 = (-SQRT_3 * u_alpha - u_beta) / 2;
    float U3 = ( SQRT_3 * u_alpha - u_beta) / 2;
    for(uint8_t i = 0; i < 8;i++)Tv[i] = 0;

    uint8_t temp = sectionCheck();		//空间矢量的扇区判断

    switch (temp){
        //计算每个扇区对应的7个空间矢量时长
        case 1:						//磁场矢量在第一扇区
            Tv[4] = K * U3;
            Tv[6] = K * U1;
            Tv[7] = Tv[0] =  (Ts - Tv[4] - Tv[6]) / 2 ;
            break;
        case 2:
            Tv[2] = -(K * U3) ;
            Tv[6] = -(K * U2);
            Tv[7] =  Tv[0] =  (Ts - Tv[2] - Tv[6]) / 2 ;
            break;
        case 3:
            Tv[2] = K * U1;
            Tv[3] = K * U2;
            Tv[7] =  Tv[0] =  (Ts - Tv[3] - Tv[2]) / 2 ;
            break;
        case 4:
            Tv[1] =  -(K * U1) ;
            Tv[3] =  -(K * U3) ;
            Tv[7] =  Tv[0] =  (Ts - Tv[1] - Tv[3]) / 2 ;
            break;
        case 5:
            Tv[1] = K * U2;
            Tv[5] = K * U3;
            Tv[7] =  Tv[0] =  (Ts - Tv[1] - Tv[5]) / 2 ;
            break;
        case 6:
            Tv[4] =  -(K * U2) ;
            Tv[5] =  -(K * U1) ;
            Tv[7] =  Tv[0] =  (Ts - Tv[4] - Tv[5]) / 2 ;
            break;
        default: break;
    }

    //因为a,b,c相都只有部分空间矢量贡献了作用时长没有贡献作用时长的时间为0，因此可以直接加起来
    uABCTime[0] = Tv[4] + Tv[5] + Tv[6] + Tv[7];		//a桥臂的时长
    uABCTime[1] = Tv[2] + Tv[3] + Tv[6] + Tv[7];
    uABCTime[2] = Tv[1] + Tv[3] + Tv[5] + Tv[7];


    __HAL_TIM_SET_COMPARE(&driver_timer,TIM_CHANNEL_1,(uint16_t)uABCTime[0]);
    __HAL_TIM_SET_COMPARE(&driver_timer,TIM_CHANNEL_2,(uint16_t)uABCTime[1]);
    __HAL_TIM_SET_COMPARE(&driver_timer,TIM_CHANNEL_3,(uint16_t)uABCTime[2]);
    __HAL_TIM_SET_COMPARE(&driver_timer,TIM_CHANNEL_4,(uint16_t)Ts - 1);

    return;
}

uint8_t FOC::sectionCheck()
{

    uint8_t section = 1;
    if (u_beta >= 0.0f) {
        if (u_alpha >= 0.0f) {
            //quadrant I
            if (one_by_sqrt3 * u_beta > u_alpha)
                section = 2; //section v2-v3
            else
                section = 1; //section v1-v2
        } else {
            //quadrant II
            if (-one_by_sqrt3 * u_beta > u_alpha)
                section = 3; //section v3-v4
            else
                section = 2; //section v2-v3
        }
    } else {
        if (u_alpha >= 0.0f) {
            //quadrant IV
            if (-one_by_sqrt3 * u_beta > u_alpha)
                section = 5; //section v5-v6
            else
                section = 6; //section v6-v1
        } else {
            //quadrant III
            if (one_by_sqrt3 * u_beta > u_alpha)
                section = 4; //section v4-v5
            else
                section = 5; //section v5-v6
        }
    }
    return section;
}